// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "serial/serial.h"
#include "std_msgs/msg/float64.hpp"
#include <iostream>
#include <cmath>

serial::Serial ros_ser;
using std::placeholders::_1;
using namespace std::chrono_literals;

// 3.自定义节点类；
class Weigh_Talker : public rclcpp::Node
{
public:
  Weigh_Talker() : Node("weigh_weigh_1node_cpp")
  {
    RCLCPP_INFO(this->get_logger(), "重量传感器发布方已创建！");
    // 发布方
    weigh_1_pub_ = this->create_publisher<std_msgs::msg::Float64>("weigh_talker_1", 10);
    weigh_2_pub_ = this->create_publisher<std_msgs::msg::Float64>("weigh_talker_2", 10);
    // 定时器
    weigh_timer = this->create_wall_timer(1ms, std::bind(&Weigh_Talker::weigh_timer_callback, this));
    // this->setzero_sub_ = this->create_subscription<std_msgs::msg::Float64>("setzero", 1, std::bind(&Weigh_Talker::zero_cb, this, std::placeholders::_1));
  }

private:
  // 定义发布方
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr weigh_1_pub_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr weigh_2_pub_;
  // 订阅方
  // rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr setzero_sub_;
  // 定义定时器
  rclcpp::TimerBase::SharedPtr weigh_timer;
  // 反馈函数
  // void zero_cb(const std_msgs::msg::Float64::SharedPtr msg)
  // {
  //   (void)msg;
  //   uint8_t senddata_1[13] = {0x01, 0x10, 0x06, 0x2a, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x01, 0x9b, 0xa8};
  //   ros_ser.write(senddata_1, 13);
  //   uint8_t senddata_2[13] = {0x02, 0x10, 0x06, 0x2a, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x01, 0x94, 0xec};
  //   ros_ser.write(senddata_2, 13);
  //   std::cout << "执行" << std::endl;
  // }
  void weigh_timer_callback()
  {
    /*
    读取参数命令
    发送： 0x01, 0x03, 0x9c, 0x40, 0x00, 0x02
    返回： 0x01, 0x03, 0x04, 0x00, 0x00, 0x0e, 0x23, 0xbf, 0x8a
    16进制数值： 0x00, 0x00, 0x0e, 0x23
    校验码：0xbf, 0x8a
    */
    uint8_t senddata[8] = {0x01, 0x03, 0x9c, 0x40, 0x00, 0x02, 0xEB, 0x8F};
    // 发送数据
    ros_ser.write(senddata, 8);
    // 创建接受数据空数组
    uint8_t readdate[9] = {0};
    // 接受数据
    ros_ser.read(readdate, 9);
    // 获取有效数据
    uint32_t data1 = ((uint32_t)readdate[6]) |
                     ((uint32_t)readdate[5] << 8) |
                     ((uint32_t)readdate[4] << 16) |
                     ((uint32_t)readdate[3] << 24);
    // std::cout << "传感器1的重量是：" << data1 << "g" << std::endl;
    std_msgs::msg::Float64 data1_pub_;
    if (data1 > std::pow(2, 32) / 2)
    {
      data1_pub_.data = data1 - std::pow(2, 32);
    }
    else
    {
      data1_pub_.data = data1;
    }
    weigh_1_pub_->publish(data1_pub_);
    // // 延迟50毫秒，等待变送器传递数据
    // std::this_thread::sleep_for(std::chrono::milliseconds(30));

    uint8_t senddata2[8] = {0x02, 0x03, 0x9c, 0x40, 0x00, 0x02, 0xEB, 0xBC};
    // 发送数据
    ros_ser.write(senddata2, 8);
    // 创建接受数据空数组
    uint8_t readdate2[9] = {0};
    // 接受数据
    ros_ser.read(readdate2, 9);
    // 获取有效数据
    uint32_t data2 = ((uint32_t)readdate2[6]) |
                     ((uint32_t)readdate2[5] << 8) |
                     ((uint32_t)readdate2[4] << 16) |
                     ((uint32_t)readdate2[3] << 24);
    // std::cout << "传感器2的重量是：" << data2 << "g" << std::endl;
    std_msgs::msg::Float64 data2_pub_;
    if (data2 > std::pow(2, 32) / 2)
    {
      data2_pub_.data = data2 - std::pow(2, 32);
    }
    else
    {
      data2_pub_.data = data2;
    }
    weigh_2_pub_->publish(data2_pub_);
    // 延迟50毫秒
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
};

int main(int argc, char const *argv[])
{
  // 2.初始化ROS2客户端；
  rclcpp::init(argc, argv);
  // 连接串口
  ros_ser.setPort("/dev/MOD_485");
  // 串口波特率
  ros_ser.setBaudrate(19200);
  serial::Timeout to = serial::Timeout::simpleTimeout(1000);
  ros_ser.setTimeout(to);
  // ros_ser.close();
  try
  {
    ros_ser.open();
  }
  catch (serial::IOException &e)
  {
    std::cout << "unable to open" << std::endl;
    return -1;
  }
  if (ros_ser.isOpen())
  {
    std::cout << "-----open------" << std::endl;
  }
  else
  {
    return -1;
  }
  // uint8_t senddata_1[13] = {0x01, 0x10, 0x06, 0x2a, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x01, 0x9b, 0xa8};
  // ros_ser.write(senddata_1, 13);
  // std::this_thread::sleep_for(std::chrono::milliseconds(30));
  // uint8_t senddata_2[13] = {0x02, 0x10, 0x06, 0x2a, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x01, 0x94, 0xec};
  // ros_ser.write(senddata_2, 13);
  // 4.调用spain函数，并传入节点对象指针；
  rclcpp::spin(std::make_shared<Weigh_Talker>());
  ros_ser.close();
  // 5.资源释放
  rclcpp::shutdown();

  return 0;
}